// #include <math.h>
#include "algo_num.h"

/**
 * @file algo_Filter.c
 * @brief Software implementation of Value calc&sort for 51 microcontroller.
 * @author David Xu
 * @version 1.0.0
 * @date 2023-11-21
 * @note none
 */

/***************************************
 * Array Statistics Functions
 ***************************************/
void ArraySet(int16_t *p, int16_t value, uint16_t arr_size) {
    uint16_t i;
    for (i = 0; i < arr_size; i++) {
        p[i] = value;
    }
}
/**
 * @brief Calculate the average of all elements in an array
 * @param p Pointer to the array
 * @param arr_size arr_size of the array
 * @return The average of all elements in the array
 */
int32_t average(int16_t *p, uint16_t arr_size) {
    int32_t  sum = 0;
    uint16_t i;

    if (arr_size == 0) { return 0; }
    for (i = 0; i < arr_size; i++) {
        sum += p[i];
    }
    return (int32_t)sum / arr_size;
}

/**
 * @brief Calculate the median of an array
 * @param p Pointer to the array
 * @param arr_size arr_size of the array
 * @return The median of the array
 */
int16_t median(int16_t *p, uint16_t arr_size) {
    int16_t i, median_val;
    int16_t local_sorted[128];    // Local array

    if (arr_size == 0) { return 0; }
    // Copy the array
    for (i = 0; i < arr_size; i++) {
        local_sorted[i] = p[i];
    }

    // Sort the array
    bubbleSortAscending(local_sorted, arr_size);

    // Calculate the median
    if (arr_size % 2 == 0) {
        median_val = (local_sorted[arr_size / 2 - 1] + local_sorted[arr_size / 2]) / 2;
    } else {
        median_val = local_sorted[arr_size / 2];
    }

    return median_val;
}

/**
 * @brief Calculate the difference between the maximum and minimum values in an array
 * @param p Pointer to the array
 * @param arr_size arr_size of the array
 * @return The difference between the maximum and minimum values in the array
 */
int16_t minmax(int16_t *p, uint16_t arr_size) {
    uint16_t i;
    int16_t  min = p[0];
    int16_t  max = p[0];

    if (arr_size == 0) { return 0; }
    for (i = 1; i < arr_size; i++) {
        if (p[i] > max) { max = p[i]; }
        if (p[i] < min) { min = p[i]; }
    }
    return max - min;
}

/**
 * @brief Calculate the variance of an array
 * @param p Pointer to the array
 * @param arr_size arr_size of the array
 * @return The variance of the array
 */
int32_t variance(int16_t *p, uint16_t arr_size) {
    int32_t  mean;
    int32_t  sum = 0;
    uint16_t i;

    if (arr_size == 0) { return 0; }
    mean = average(p, arr_size);
    for (i = 0; i < arr_size; i++) {
        sum += (p[i] - mean) * (p[i] - mean);
    }
    return sum / arr_size;
}

/**
 * @brief Calculate the covariance of two arrays
 * @param p1 Pointer to the first array
 * @param p2 Pointer to the second array
 * @param arr_size arr_size of the arrays
 * @return The covariance of the two arrays
 */
int32_t covariance(int16_t *p1, int16_t *p2, uint16_t arr_size) {
    int32_t  mean1, mean2, sum = 0;
    uint16_t i;
    if (arr_size == 0) { return 0; }
    mean1 = average(p1, arr_size);
    mean2 = average(p2, arr_size);

    for (i = 0; i < arr_size; i++) {
        sum += (p1[i] - mean1) * (p2[i] - mean2);
    }
    return sum / arr_size;
}
/**
 * @brief Calculate the standard deviation of an array
 * @param p Pointer to the array
 * @param arr_size arr_size of the array
 * @return The standard deviation of the array
 */
#if 0
int32_t stddev(int16_t *p, uint16_t arr_size) {
    int32_t  mean, sum = 0;
    uint16_t i;
    if (arr_size == 0) { return 0; }
    mean = average(p, arr_size);

    for (i = 0; i < arr_size; i++) {
        sum += (p[i] - mean) * (p[i] - mean);
    }
    return sqrt(sum / arr_size);
}
#else
int32_t sqrt_newton(int32_t num) {
    int32_t x = num;
    int32_t y = (x + 1) >> 1;
    while (y < x) {
        x = y;
        y = ((y + (num / y)) >> 1);
    }
    return x;
}

int32_t stddev(int16_t *p, uint16_t arr_size) {
    int32_t  mean, sum = 0;
    int16_t  temp;
    uint16_t i;
    if (arr_size == 0) { return 0; }
    mean = average(p, arr_size);    // Make sure average() function is optimized for integer operation

    for (i = 0; i < arr_size; i++) {
        temp = p[i] - mean;
        sum += (int32_t)temp * temp;    // Use right shift to avoid overflow and simulate division by 2^16
    }
    sum /= arr_size;            // Integer division
    return sqrt_newton(sum);    // Use the integer square root approximation
}

#endif

/**
 * @brief Calculate the quotient of two integers and map the result from [0,1] to [0,256]
 * @param dividend The dividend
 * @param divisor The divisor
 * @return The quotient mapped to the range [0,256]
 */
uint8_t Percent2Byte(int16_t dividend, int16_t divisor) {
    int16_t quotient;
    if (divisor == 0) {
        // Avoid division by zero error
        return 0;
    }
    quotient = ((int32_t)dividend << 8) / divisor;
    if (quotient < 0) {
        // If the quotient is negative, return 0
        return 0;
    } else if (quotient > 255) {
        return 255;
    } else {
        return (uint8_t)quotient;
    }
}

/**
 * @brief Calculate the quotient of the value times the factor
 * @param value The value
 * @param percentTimes256 The percent times 256
 * @return The quotient mapped to the range [0, 256]
 */
int16_t Percent2Result(int16_t value, uint8_t percentTimes256) {
    int16_t quotient;
    if (percentTimes256 == 2) {
        quotient = value >> 7;
    } else if (percentTimes256 == 4) {
        quotient = value >> 6;
    } else if (percentTimes256 == 8) {
        quotient = value >> 5;
    } else if (percentTimes256 == 16) {
        quotient = value >> 4;
    } else if (percentTimes256 == 32) {
        quotient = value >> 3;
    } else if (percentTimes256 == 64) {
        quotient = value >> 2;
    } else if (percentTimes256 == 128) {
        quotient = value >> 1;
    } else if (percentTimes256 == 255) {
        quotient = value;
    } else {
        quotient = ((int32_t)value * percentTimes256) >> 8;
    }
    return quotient;
}

/**
 * @brief Calculate the log2 of a number
 * @param inVal The number
 * @return The log2 of the number
 */
uint32_t uint32_log2(uint32_t inVal) {
    uint32_t tempOut = 0;
    if (inVal == 0)
        return 0;

    if (inVal >= (1UL << 16)) {
        inVal >>= 16;
        tempOut += 16;
    }
    if (inVal >= (1 << 8)) {
        inVal >>= 8;
        tempOut += 8;
    }
    if (inVal >= (1 << 4)) {
        inVal >>= 4;
        tempOut += 4;
    }
    if (inVal >= (1 << 2)) {
        inVal >>= 2;
        tempOut += 2;
    }
    if (inVal >= (1 << 1)) { tempOut += 1; }
    return tempOut;
}

/***************************************
 * Array Sorting Functions
 ***************************************/

/**
 * @brief Sorts an array of integers in ascending order using bubble sort algorithm.
 * @param arr The array to be sorted.
 * @param arr_size The size of the array.
 */
void bubbleSortAscending(int16_t arr[], uint16_t arr_size) {
    uint16_t i, j;
    int16_t  tmp;
    for (i = 0; i < arr_size - 1; i++) {
        for (j = 0; j < arr_size - i - 1; j++) {
            if (arr[j] > arr[j + 1]) {
                tmp        = arr[j];
                arr[j]     = arr[j + 1];
                arr[j + 1] = tmp;
            }
        }
    }
}

/**
 * @brief Sorts an array of integers in descending order using bubble sort algorithm.
 * @param arr The array to be sorted.
 * @param arr_size The size of the array.
 */
void bubbleSortDescending(int16_t arr[], uint16_t arr_size) {
    uint16_t i, j;
    int16_t  tmp;
    for (i = 0; i < arr_size - 1; i++) {
        for (j = 0; j < arr_size - i - 1; j++) {
            if (arr[j] < arr[j + 1]) {
                tmp        = arr[j];
                arr[j]     = arr[j + 1];
                arr[j + 1] = tmp;
            }
        }
    }
}

/**
 * @brief Sorts an array of integers in ascending order using insertion sort algorithm.
 * @param arr The array to be sorted.
 * @param arr_size The size of the array.
 */
void insertSortAscending(int16_t arr[], uint16_t arr_size) {
    uint16_t i;
    int16_t  j, key;
    for (i = 1; i < arr_size; i++) {
        key = arr[i];
        j   = i - 1;
        while (j >= 0 && arr[j] > key) {
            arr[j + 1] = arr[j];
            j--;
        }
        arr[j + 1] = key;
    }
}

/**
 * @brief Sorts an array of integers in descending order using insertion sort algorithm.
 * @param arr The array to be sorted.
 * @param arr_size The size of the array.
 */
void insertSortDescending(int16_t arr[], uint16_t arr_size) {
    uint16_t i;
    int16_t  j, key;
    for (i = 1; i < arr_size; i++) {
        key = arr[i];
        j   = i - 1;
        while (j >= 0 && arr[j] < key) {
            arr[j + 1] = arr[j];
            j--;
        }
        arr[j + 1] = key;
    }
}

/***************************************
 * Array Search Functions
 ***************************************/

/**
 * @brief  Binary search function to locate a given value's position in a table.
 *
 * @param  value         The value to search for.
 * @param  table         The table/array in which to search.
 * @param  arr_size          arr_size of the table/array.
 * @param  isDescending  Indicates if the table is arranged in descending order.
 *                       1 for descending and 0 for ascending.
 *
 * @return The index of the found value. If an exact match isn't found,
 *         it returns the closest index.
 */
uint16_t BinarySearch(uint16_t value, uint16_t *table, uint16_t arr_size, bool isDescending) {
    uint16_t top  = arr_size - 1;
    uint16_t base = 0;
    uint16_t mid  = 0;

    while (base <= top) {
        mid = (top + base) / 2;

        if (table[mid] == value) return mid;

        if (isDescending) {
            if (table[mid] < value)
                top = mid - 1;
            else
                base = mid + 1;
        } else {
            if (table[mid] > value)
                top = mid - 1;
            else
                base = mid + 1;
        }
    }
    return mid;
}

#if 0
unsigned int arr1[] = { 10, 20, 30, 40, 50 };
unsigned int arr2[] = { 10, 10, 10, 10, 10 };

void test_sqrt_newton() {
    printf("Testing sqrt_newton...\n");
    printf("sqrt_newton(4) = %d\n", (int)sqrt_newton(4));      // Expected output: 2
    printf("sqrt_newton(9) = %d\n", (int)sqrt_newton(9));      // Expected output: 3
    printf("sqrt_newton(16) = %d\n", (int)sqrt_newton(16));    // Expected output: 4
}

void test_stddev() {

    printf("Testing stddev...\n");

    printf("stddev(arr1) = %d\n", (int)stddev(arr1, 5));    // Expected output: 1 (or close to 1)

    printf("stddev(arr2) = %d\n", (int)stddev(arr2, 5));    // Expected output: 0
}

void test_num(void) {
    test_sqrt_newton();
    test_stddev();
}

#endif
